#include "functionsse.h"

// ================================================================================================
// SSE dot product 
// ================================================================================================
__m128 DotProductSSE(const __m128& v1, const __m128& v2) {
    __m128 m = _mm_mul_ps(v1, v2);
    __m128 t = _mm_add_ps(m, _mm_shuffle_ps(m, m, _MM_SHUFFLE(2,3, 0,1)));
    __m128 result = _mm_add_ps(t, _mm_shuffle_ps(t, t, _MM_SHUFFLE(1, 0, 3, 2)));

    return result;
}

// ================================================================================================
// SSE dot product using data parallelism
// ================================================================================================
__m128 DotProductSSE4x4(const __m128 v1[4], const __m128 v2[4]) {
    __m128 x = _mm_mul_ps(v1[0], v2[0]);
    __m128 y = _mm_mul_ps(v1[1], v2[1]);
    __m128 z = _mm_mul_ps(v1[2], v2[2]);
    __m128 w = _mm_mul_ps(v1[3], v2[3]);

    __m128 xy = _mm_add_ps(x, y);
    __m128 zw = _mm_add_ps(z, w);

    __m128 xyzw = _mm_add_ps(xy, zw);

    return xyzw;
}

// ================================================================================================
// Add using SSE using data parallelism
// ================================================================================================
void AddSSE4x4(const __m128 v1[4], const __m128 v2[4], __m128 res[4]) {
    res[0] = _mm_add_ps(v1[0], v2[0]); 
    res[1] = _mm_add_ps(v1[1], v2[1]); 
    res[2] = _mm_add_ps(v1[2], v2[2]); 
    res[3] = _mm_add_ps(v1[3], v2[3]); 
}

// ================================================================================================
// Multiply using SSE using data parallelism
// ================================================================================================
void MulSSE4x4(const __m128 v1[4], const __m128 v2[4], __m128 res[4]) {
    res[0] = _mm_mul_ps(v1[0], v2[0]);
    res[1] = _mm_mul_ps(v1[1], v2[1]);
    res[2] = _mm_mul_ps(v1[2], v2[2]);
    res[3] = _mm_mul_ps(v1[3], v2[3]);
}

// ================================================================================================
// Multiply using SSE 1x4
// ================================================================================================
void MulSSE1x4(const __m128& v1, const __m128 v2[4], __m128 res[4]) {
    res[0] = _mm_mul_ps(v1, v2[0]);
    res[1] = _mm_mul_ps(v1, v2[1]);
    res[2] = _mm_mul_ps(v1, v2[2]);
    res[3] = _mm_mul_ps(v1, v2[3]);
}

// ================================================================================================
// Multiply using SSE 4x1
// ================================================================================================
void MulSSE4x1(const __m128 v1[4], const __m128& v2, __m128 res[4]) {
    res[0] = _mm_mul_ps(v2, v1[0]);
    res[1] = _mm_mul_ps(v2, v1[1]);
    res[2] = _mm_mul_ps(v2, v1[2]);
    res[3] = _mm_mul_ps(v2, v1[3]);
}

// ================================================================================================
// Find the fabs of the __m128 val
// ================================================================================================
__m128 FAbsSSE(const __m128 val) {
    static const __m128 sign_mask = _mm_set1_ps(-0.f);
    return _mm_andnot_ps(sign_mask, val);

}
